/*
 * File:    ModAscRtu.c
 * $Author: CreMindES@gmail.com $
 *
 * $Revision: 21 $
 * $Date: 2014-02-16 11:20:40 +0000 (Sun, 16 Feb 2014) $
 *
 */

#include <xc.h>
#include <math.h>
#include <pic18f46k22.h>

#include "crc_source.h"
#include "modascrtu.h"

#pragma config LVP = OFF
//#pragma config FOSC = INTIO67                   // Internal Oscillator
#pragma config FOSC = HSHP
#pragma config WDTEN = OFF                      // Watchdog off
#pragma config DEBUG = OFF
//#pragma config MCLRE = ON


int main(void)
{
    initDevice();
    USART1_RX_LED_ON();
    initUsart1();
    USART1_TX_LED_ON();
    initUsart2();
    USART2_RX_LED_ON();
    initTimer0();
    USART2_TX_LED_ON();
    turnLEDsOff();

    while(1) {
        switch(communicationState) {
            case idle: {

                break;
            }
            case receivingAsciiMessage: {
                receiveAsciiMessage();
                break;
            }
            case receivingRtuMessage: {
                receiveRtuMessage();
                break;
            }
            case convertingDataFormat: {
                switch(messageAvailability) {
                    case asciiMessageIsAvailable: {
                        if( convertAsciiMessageToRtu() ) {
                            messageAvailability = noMessageIsAvailable;
                            txBufferState = txRtuMessageAvailable;
                            communicationState = sendingMessage;
                        }
                        else {
                            communicationState = error;
                        }
                        break;
                    }
                    case rtuMessageIsAvailable: {
                        if( convertRtuMessageToAscii() ) {
                            messageAvailability = noMessageIsAvailable;
                            txBufferState = txAsciiMessageAvailable;
                            communicationState = sendingMessage;
                        }
                        else {
                            communicationState = error;
                        }
                        break;
                    }
                    case noMessageIsAvailable: {
                        errorState = unknownError;
                        communicationState = error;
                        break;
                    }
                    default: {
                        errorState = unknownError;
                        communicationState = error;
                    }
                }
                break;
            }
            case sendingMessage: {
                if(txBufferState == txAsciiMessageAvailable) {
                    if(usart1Configuration.transmissionMode == ASCII_MODE) {
                        sendUsart1Data();
                    }
                    else {
                        sendUsart2Data();
                    }
                }
                if( txBufferState == txRtuMessageAvailable) {
                    if(usart1Configuration.transmissionMode == RTU_MODE) {
                        sendUsart1Data();
                    }
                    else {
                        sendUsart2Data();
                    }
                }

                resetCommunication();

                break;
            }
            case error: {
                USART1_RX_LED_ON();
                resetCommunication();
                break;
            }
            default: {
                USART1_RX_LED_ON();
            }

        }
    }

    // checkBuffers();

    return;
}

void initDevice(void)
{
    // For internal Oscillator
/*
    OSCCONbits.IRCF0 = 1;                   // HFINTOSC ? (16 MHz)
    OSCCONbits.IRCF1 = 1;
    OSCCONbits.IRCF2 = 1;
    OSCCONbits.SCS0 = 1;                    // System clock is the internal OSC
    OSCCONbits.SCS1 = 1;
    OSCTUNE = 0x00;                         // Default, not tuning OSC module
    OSCTUNEbits.PLLEN = 1;                  // Disable Phase Lock Loop
*/

/*
    // Disable all unused peripheral modules
    PMD0bits.TMR6MD = 1;
    PMD0bits.TMR5MD = 1;
    PMD0bits.TMR4MD = 1;
    PMD0bits.TMR3MD = 1;
    PMD0bits.TMR2MD = 1;
    PMD1 = 0xFF;
    PMD2 = 0xFF;
*/

    // General IO
    TRISBbits.TRISB3 = 0;                   // Power LED
    PORTBbits.RB3 = 1;                      // Turn on Power LED
    
    TRISDbits.TRISD0 = 0;                   // LED2
    TRISDbits.TRISD1 = 0;                   // LED3
    TRISDbits.TRISD2 = 0;                   // LED4
    TRISDbits.TRISD3 = 0;                   // LED5

    TRISAbits.RA0 = 1;                      // DIP1 
    TRISAbits.RA1 = 1;                      // DIP2
    TRISAbits.RA2 = 1;                      // DIP3
    TRISAbits.RA3 = 1;                      // DIP4
    TRISAbits.RA4 = 1;                      // DIP5
    TRISAbits.RA5 = 1;                      // DIP6
    TRISEbits.RE0 = 1;                      // DIP7
    TRISEbits.RE1 = 1;                      // DIP8

    ANSELAbits.ANSA0 = 0;
    ANSELAbits.ANSA1 = 0;
    ANSELAbits.ANSA2 = 0;
    ANSELAbits.ANSA3 = 0;
    //ANSELAbits.ANSA4 = 0;                 // missing from header file, should report to Microchip
    ANSELAbits.ANSA5 = 0;
    ANSELEbits.ANSE0 = 0;
    ANSELEbits.ANSE1 = 0;


    // Configuring Interrupts
    INTCON = 0;                             // Disable all interrupt
    INTCON2 = 0;
    INTCON3 = 0;
    PIE1 = 0;
    PIE2 = 0;
    PIE3 = 0;
    PIE4 = 0;
    PIE5 = 0;

    INTCONbits.GIE = 1;                     // Global Interrupt Enabled
    INTCONbits.PEIE = 1;		    // Peripheral Interrupt Enabled

    readConfiguration();                    // Set up switchable functions

    return;
}

void readConfiguration(void)
{
    #ifdef TRANS_MODE_SELECTION
    // USART1
    if(!PORTAbits.RA0) usart1Configuration.transmissionMode = RTU_MODE;
        else usart1Configuration.transmissionMode = ASCII_MODE;           //not neccesary line
    #endif
    if(!PORTAbits.RA1) usart1BaudRateTemp |= 1;
    if(!PORTAbits.RA2) usart1BaudRateTemp |= 1<<1;
    switch(usart1BaudRateTemp) {
        case 0: {
            usart1Configuration.baudRate = B_2400;
            rtuValidCharTimeoutOffset = RtuCharTimeout_baud2400;
            break;
        }
        case 1: {
            usart1Configuration.baudRate = B_9600;
            rtuValidCharTimeoutOffset = RtuCharTimeout_baud9600;
            break;
        }
        case 2: {
            usart1Configuration.baudRate = B_19200;
            rtuValidCharTimeoutOffset = RtuCharTimeout_baud19200;
            break;
        }
        case 3: {
            usart1Configuration.baudRate = B_38400;
            rtuValidCharTimeoutOffset = RtuCharTimeout_baud38400;
            break;
        }
    }

    #ifdef TRANS_MODE_SELECTION
    // USART2
    if(!PORTAbits.RA3) usart2Configuration.transmissionMode = RTU_MODE;
        else usart2Configuration.transmissionMode = ASCII_MODE;           //not neccesary line
    #endif
    if(!PORTAbits.RA4) usart2BaudRateTemp |= 1;
    if(!PORTAbits.RA5) usart2BaudRateTemp |= 1<<1;
    switch(usart2BaudRateTemp) {
        case 0: {
            usart2Configuration.baudRate = B_2400;
            rtuValidCharTimeoutOffset = RtuCharTimeout_baud2400;
            break;
        }
        case 1: {
            usart2Configuration.baudRate = B_9600;
            rtuValidCharTimeoutOffset = RtuCharTimeout_baud9600;
            break;
        }
        case 2: {
            usart2Configuration.baudRate = B_19200;
            rtuValidCharTimeoutOffset = RtuCharTimeout_baud19200;
            break;
        }
        case 3: {
            usart2Configuration.baudRate = B_38400;
            rtuValidCharTimeoutOffset = RtuCharTimeout_baud38400;
            break;
        }
    }

}

void initTimer0 (void)
{
    T0CONbits.T08BIT = 1;                   // Timer0 is configured as an 16-bit timer/counter
    T0CONbits.T0CS = 0;                     // Internal instruction cycle clock
    T0CONbits.T0SE = 1;                     // Increment on high-to-low transition on T0CKI pin
    T0CONbits.PSA = 0;                      // Timer0 prescaler is assigned. Timer0 clock input comes from prescaler output
    T0CONbits.T0PS2 = 1;                    // Prescale set to 256
    T0CONbits.T0PS1 = 1;
    T0CONbits.T0PS0 = 1;
    T0CONbits.TMR0ON = 0;                   // Disable Timer0
    return;
}

void initTimer1(void)
{
    T1CONbits.TMR1CS0 = 0;                      // Timer1/3/5 clock source is instruction clock (FOSC/4)
    T1CONbits.TMR1CS1 = 0;
    T1CONbits.T1CKPS0 = 1;                      // Prescale 8
    T1CONbits.T1CKPS1 = 1;
    T1CONbits.T1SOSCEN = 0;                     // Dedicated Secondary oscillator circuit disabled
    T1CONbits.T1SYNC = 0;                       // Do not synchronize external clock input
    T1CONbits.T1RD16 = 1;                       // Enables register read/write of Timer1/3/5 in one 16-bit operation
    T1CONbits.TMR1ON = 0;                       // Clears Timer1/3/5

    PIE1bits.TMR1IE = 0;                        // Disable Timer 1 interrupt

    return;
}

void turnLEDsOff(void)
{
    USART1_RX_LED_OFF();
    USART1_TX_LED_OFF();
    USART2_RX_LED_OFF();
    USART2_TX_LED_OFF();
}

void interrupt myISR(void) // TODO: clear this up
{
    if(RCSTA1bits.OERR1 || RCSTA1bits.FERR1) {
        INTCONbits.GIE = 0;
        INTCONbits.TMR0IF = 0;
        T0CONbits.TMR0ON = 0;
        DISABLE_USART1_RECEPTION();                             // disable interrupt on receive
        DISABLE_USART2_RECEPTION();
        //LATD0 = 1;
        //for(int i=0; i<30; ++i) __delay_ms(10);
        RCSTA1bits.SPEN = 0;
        RCSTA1bits.SPEN = 1;
        resetCommunication();
        //LATD0 = 0;
        INTCONbits.GIE = 1;
        return;
    }
    if(PIR1bits.RC1IF) {
        // TODO: start a timer
        //startTimeoutCheckService();

        USART1_RX_LED_ON();
        usart1RxBuffer[usart1RxCount++] = RCREG1;
        USART1_RX_LED_OFF();

        if(usart1Configuration.transmissionMode == ASCII_MODE) {
            if(communicationState != receivingAsciiMessage) {
                communicationState = receivingAsciiMessage;
            }
            return;
        }
        else {                                              // RTU mode
            isRtuByteAvailable = 1;
            if(communicationState != receivingRtuMessage) {
                startReceivingRtuMessage();
            }
        }
        return;
    }

    if(RCSTA2bits.OERR2 || RCSTA2bits.FERR2) {
        INTCONbits.GIE = 0;
        INTCONbits.TMR0IF = 0;
        T0CONbits.TMR0ON = 0;
        DISABLE_USART1_RECEPTION();                         // disable interrupt on receive
        DISABLE_USART2_RECEPTION();
        //LATD3 = 1;
        //for(int i=0; i<30; ++i) __delay_ms(10);
        RCSTA2bits.SPEN = 0;
        RCSTA2bits.SPEN = 1;
        resetCommunication();
        //LATD3 = 0;
        INTCONbits.GIE = 1;
        return;
    }

    if(PIR3bits.RC2IF) {
        USART2_RX_LED_ON();
        usart2RxBuffer[usart2RxCount++] = RCREG2;
        USART2_RX_LED_OFF();

        if(usart2Configuration.transmissionMode == ASCII_MODE) {
            if(communicationState != receivingAsciiMessage) {
                communicationState = receivingAsciiMessage;
            }
            return;
        }
        else {                                              // RTU mode
            isRtuByteAvailable = 1;
            
            if(communicationState != receivingRtuMessage) {
                startReceivingRtuMessage();
            }
        }
        return;
        
//        return;
    }

    if (INTCONbits.TMR0IF) {
        INTCONbits.TMR0IE = 0;
        INTCONbits.TMR0IF = 0;
        T0CONbits.TMR0ON = 0;
        
        messageAvailability = rtuMessageIsAvailable;

        return;
    }

    /*
    if(PIR1bits.TMR1IF) {
        PIR1bits.TMR1IF = 0;
        timeoutCount++;
        if(timeoutCount == 65000) {
            timeoutCount = 0;
            resetCommunication();
            stopTimeoutCheckService();
            modbusTxBuffer[0] = 0;
            modbusTxCount = 1;
            sendModbusData();       
        }
        else return;
        */
        /*
        timeoutCount++;
        if(timeoutCount == 24000) {             // recalculate!!!!!!!
            LATD ^=  (1<<3);
            timeoutCount = 0;
        }
        */
    /*
    }*/

   // debugDummy = 1;

    return;
}

void resetCommunication(void) {     // TODO: recheck and complete is necessary
    //stopTimeoutCheckService();

    usart1RxCount = 0;
    usart1TxCount = 0;

    usart2RxCount = 0;
    usart2TxCount = 0;

    // Reset device state
    communicationState = idle;
    messageAvailability = noMessageIsAvailable;
    txBufferState = txEmpty;
    errorState = noError;
    isRtuByteAvailable = 0;

    //RCSTA1bits.CREN = 0;
    //RCSTA1bits.CREN = 1;
    //RCSTA2bits.CREN = 0;
    //RCSTA2bits.CREN = 1;

    switchUsart1ToRx();
    switchUsart2ToRx();

    /*
    //flushUSART1
    flush = RCREG1;
    flush = RCREG1;

    //flushUSART2
    flush = RCREG2;
    flush = RCREG2;
    */

    ENABLE_USART1_RECEPTION();
    ENABLE_USART2_RECEPTION();

    return;
}

// Todo: implement something similar
void startTimeoutCheckService(void) {
    if(T1CONbits.TMR1ON) TMR1 = 0;
    else {
        T1CONbits.TMR1ON = 1;
        PIE1bits.TMR1IE = 1;
        TMR1 = 0;
    }
    return;
}

void stopTimeoutCheckService(void) {
    PIE1bits.TMR1IE = 0;
    T1CONbits.TMR1ON = 0;
    return;
}

void switchUsart1ToTx(void)
{
    //TXSTA1bits.TXEN = 1;            // Add delay?
    //Delay10TCY();
    PORTDbits.RD4 = 1;
    return;
}

void switchUsart1ToRx(void) {
    PORTDbits.RD4 = 0;
    //TXSTA1bits.TXEN = 0;
    RCSTA1bits.SPEN = 0;
    RCSTA1bits.SPEN = 1;
}

void initUsart1(void)
{
    // EUSART 1

    // Setting up IO ports
    TRISCbits.RC6 = 0;                  // Set RC6 as output for TX1
    ANSELCbits.ANSC6 = 0;               // Set RC6 as Trasmitter output

    TRISCbits.RC7 = 1;                  // Set RC7 as input for RX1
    ANSELCbits.ANSC7 = 0;               // Set RC7 as Receiver input

    TRISDbits.TRISD4 = 0;               // Set RD4 as output for Modbus TX mod
    ANSELDbits.ANSD4 = 1;               // Set RD4 as LATD4 output, disable analog

    // Configuring EUSART1
    SPBRG1 = usart1Configuration.baudRate & 0xFF;   // Setting the desired baudrate
    SPBRGH1 = usart1Configuration.baudRate >> 8;    // Only used, if BRG16 is set
    TXSTA1bits.BRGH = 0;                            // Low Speed mode
    BAUDCON1bits.CKTXP = 0;                         // Normal TX Data Polarity
    BAUD1CONbits.DTRXP = 0;                         // Normal RX Data Polarity
    BAUDCON1bits.BRG16 = 1;                         // 8-bit Baud Rate Generator is used
    BAUDCON1bits.ABDEN = 0;                         // Disable Autobaud detection
    BAUD1CONbits.WUE = 0;                           // Disable Wake up on receive

    RCSTA1bits.SPEN = 1;                            // Serial Port Enable bit
    RCSTA1bits.CREN = 1;                            // Enable the reception
    RCSTA1bits.RX9 = 0;                             // 8 bit mode
    RCSTA1bits.ADDEN = 0;                           // Address Detection, here don't care

    TXSTA1bits.SYNC = 0;                            // Asynchronous mode
    TXSTA1bits.TXEN = 1;                            // Transmit enabled
    TXSTA1bits.TX9 = 0;                             // 8 bit transmission mode
    TXSTA1bits.SENDB = 0;                           // don't care

    PIE1bits.TX1IE = 0;                             // Disable interrupt on transmit
    PIE1bits.RC1IE = 1;                             // Enable interrupt on receive

    return;
}

void initUsart2(void)
{
    // EUSART 2

    // Setting up IO ports
    TRISDbits.RD6 = 0;                  // Set RD6 as output for TX2
    ANSELDbits.ANSD6 = 0;               // Set RD6 as Trasmitter output

    TRISDbits.RD7 = 1;                  // Set RD7 as input for RX2
    ANSELDbits.ANSD7 = 0;               // Set RD7 as Receiver input

    TRISDbits.TRISD5 = 0;               // Set RD5 as output for K-Net TX mod
    ANSELDbits.ANSD5 = 1;               // Set RD5 as LATD5 output, disable analog

    // Configuring EUSART2
    SPBRG2 = usart2Configuration.baudRate & 0xFF;   // Setting the desired baudrate
    SPBRGH2 = usart2Configuration.baudRate >> 8;    // Not used as BRG16 = 0
    TXSTA2bits.BRGH = 0;                            // Low Speed mode
    BAUDCON2bits.CKTXP = 0;                         // Normal TX Data Polarity
    BAUDCON2bits.DTRXP = 0;                         // Normal RX Data Polarity
    BAUDCON2bits.BRG16 = 1;                         // 8-bit Baud Rate Generator is used
    BAUDCON2bits.ABDEN = 0;                         // Disable Autobaud detection
    BAUDCON2bits.WUE = 0;                           // Disable Wake up on receive

    RCSTA2bits.SPEN = 1;                            // Serial Port Enable bit
    RCSTA2bits.CREN = 1;                            // Enable the reception
    RCSTA2bits.RX9 = 0;                             // 8 bit mode
    RCSTA2bits.ADDEN = 0;                           // Address Detection, here don't care

    TXSTA2bits.SYNC = 0;                            // Asynchronous mode
    TXSTA2bits.TXEN = 1;                            // Transmit enabled
    TXSTA2bits.TX9 = 0;                             // 8 bit transmission mode
    TXSTA2bits.SENDB = 0;                           // don't care

    PIE3bits.TX2IE = 0;                             // Disable interrupt on transmit
    PIE3bits.RC2IE = 1;                             // Enable interrupt on receive

    return;
}

void switchUsart2ToTx(void)
{
    PORTDbits.RD5 = 1;
    return;
}

void switchUsart2ToRx(void) {
    PORTDbits.RD5 = 0;
    RCSTA2bits.SPEN = 0;
    RCSTA2bits.SPEN = 1;
    return;
}

void receiveAsciiMessage(void)
{
    if(asciiRxBuffer[(*asciiRxCount)-1] == 0x0A) {          // Is current char Modbus ASCII ending char ':' ?
        DISABLE_USART1_RECEPTION();                         // disable interrupt on receive
        DISABLE_USART2_RECEPTION();                         // TODO: recheck logic + seprate function?
        //asciiRxBuffer[(*asciiRxCount)-1] = 9;             // dummy char -> not neccesary in this configuration

        if(validateAsciiMessage()) {                        // validating ASCII Message format
            messageAvailability = asciiMessageIsAvailable;  // Ascii message is available for convertion
            communicationState = convertingDataFormat;      // switching to converting status (ASCII -> RTU)
        }
        else {
            errorState = invalidAsciiMessage;
            communicationState = error;
        }
    }
    else {                                                  // Message is still coming in
        // PIR1bits.RCIF = 1;                               // generate interrupt for next char
        communicationState = idle;
        return;
    }   

    return;
}

void startReceivingRtuMessage(void)
{
    communicationState = receivingRtuMessage;
    T0CONbits.TMR0ON = 1;                                   // Set Timer0 ON
    INTCONbits.TMR0IE = 1;                                  // Enable Timer0 interrupt
    TMR0 = 0;
    return;
}

void receiveRtuMessage(void)
{
    if(isRtuByteAvailable) {
        isRtuByteAvailable = 0;
        waitForNextRtuCharacter();
    }

    else if(messageAvailability == rtuMessageIsAvailable) {
        DISABLE_USART1_RECEPTION();                         // disable interrupt on receive
        DISABLE_USART2_RECEPTION();

        if(validateRtuMessage()) {
            communicationState = convertingDataFormat;
        }
        else {
            errorState = invalidRtuMessage;
            communicationState = error;
        }
    }
    else {
        
    }
    
    return;
}

void waitForNextRtuCharacter(void)
{
    //T0CONbits.TMR0ON = 1;                                   // Set Timer0 ON
    //INTCONbits.TMR0IE = 1;                                  // ReEnable Timer0 interrupts
    TMR0 = rtuValidCharTimeoutOffset;                         // preset timeout value
    return;
}

unsigned char convertAsciiMessageToRtu(void)
{
    unsigned char i;
    unsigned char messageLenght;
    unsigned int crc;

    messageLenght = (*asciiRxCount) - 5;

    for(i=1; i<=messageLenght; i+=2) {
            rtuTxBuffer[(*rtuTxCount)++] = convertAsciiToRtu(asciiRxBuffer, i);
    }

    crc = generateCRC(*rtuTxCount, rtuTxBuffer);

    rtuTxBuffer[(*rtuTxCount)++] = crc >> 8;
    rtuTxBuffer[(*rtuTxCount)++] = crc;

    *asciiRxCount = 0;
  
    return 1;
}

unsigned char convertAsciiToRtu(unsigned char *asciiBuffer, unsigned char byteNumber)
{
    int rtuByte;

    if(asciiBuffer[byteNumber] < 58) rtuByte = (asciiBuffer[byteNumber] - 48) * 16;
    else rtuByte = (asciiBuffer[byteNumber] - 55) * 16;
    if(asciiBuffer[byteNumber+1] < 58) rtuByte += (asciiBuffer[byteNumber+1] - 48);
    else rtuByte += (asciiBuffer[byteNumber+1] - 55);

    return rtuByte & 255;
}

unsigned char convertRtuMessageToAscii(void)
{
    unsigned char i;
    unsigned char messageLenght;                // subtract the 2 byte CRC from the end
    unsigned char lrc;

    messageLenght = *rtuRxCount;
    messageLenght -= 2;

    asciiTxBuffer[(*asciiTxCount)++] = ':';

    for(i=0; i<messageLenght; i++) {
            asciiTxBuffer[(*asciiTxCount)++] = convertHexToAsciiHi(rtuRxBuffer[i]);
            asciiTxBuffer[(*asciiTxCount)++] = convertHexToAsciiLo(rtuRxBuffer[i]);
    }

    lrc = generateLRC((*asciiTxCount)-1, asciiTxBuffer);

    asciiTxBuffer[(*asciiTxCount)++] = convertHexToAsciiHi(lrc);
    asciiTxBuffer[(*asciiTxCount)++] = convertHexToAsciiLo(lrc);

    asciiTxBuffer[(*asciiTxCount)++] = 0x0D;
    asciiTxBuffer[(*asciiTxCount)++] = 0x0A;

    return 1;
}

unsigned int generateCRC(unsigned char len, unsigned char *buffer)      	// from modbus documentation
{
	//unsigned short len = rxCount;						// quantity of bytes in message
	unsigned char uchCRCHi = 0xFF; 						// high CRC byte initialized
	unsigned char uchCRCLo = 0xFF; 						// low CRC byte initialized
	unsigned int crc;
	unsigned char uIndex, i=0; 						// will index into CRC lookup table
	while (len--) { 							// pass through message buffer
		uIndex = uchCRCHi ^ buffer[i];   				// calculate the CRC
		uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex];
                uchCRCLo = auchCRCLo[uIndex];
		i++;
	}
	crc = uchCRCHi;
	crc <<= 8;
	crc += uchCRCLo;
	return crc;
}

unsigned char generateLRC(unsigned char len, unsigned char *buffer)
{
    short int i, g, h, lrc=0;
    for(i=1; i<len; i+=2) {                                                     // converting dec num in char to hex
        g = 0;
        h = 0;
        if(buffer[i] < 58) g += (buffer[i] - 48) * 16;
        else g += (buffer[i] - 55) * 16;
        if(buffer[i+1] < 58) h += (buffer[i+1] - 48);
        else h += (buffer[i+1] - 55);                                           // converting dec num in char to hex
        lrc += (g + h) & 255;
    }
    return (-(lrc)) & 255;
}

unsigned char validateAsciiMessage(void)
{
	unsigned int lrc = generateLRC((*asciiRxCount)-4, asciiRxBuffer);
	if(asciiRxBuffer[0] != ':') return 0;
	//if(asciiRxBuffer[(*asciiRxCount)-1] != 0x0A) return 0;                    // not neccesary in this configuration
        if(convertDecToHexInAsciiHi(lrc) != asciiRxBuffer[(*asciiRxCount)-4] ||
           convertDecToHexInAsciiLo(lrc) != asciiRxBuffer[(*asciiRxCount)-3]) {
           return 0;
        }
	return 1;
}

unsigned char validateRtuMessage(void)
{
	unsigned int crc;

	crc = generateCRC((*rtuRxCount)-2, rtuRxBuffer);

	if( (*rtuRxCount) < 7 ) return 0;                                       // if message is smaller than minimum lenght
	if( (char)(crc>>8) != rtuRxBuffer[(*rtuRxCount)-2] ||
            (char)crc != rtuRxBuffer[(*rtuRxCount)-1] ) {
                return 0;                                                     // if CRC error | reversed order!
        }

	return 1;
}

void sendUsart1Data(void)
{
    unsigned char i = 0;
    
    switchUsart1ToTx();
    USART1_TX_LED_ON();
    for(i=0; i<usart1TxCount; ++i) {
        TXREG1 = usart1TxBuffer[i];
        while(!TXSTA1bits.TRMT1);
    }
    usart1TxCount = 0;
    USART1_TX_LED_OFF();
    switchUsart1ToRx();
    //stopTimeoutCheckService(); // Timeout service stop
    //TODO: check back

    return;
}

void sendUsart2Data(void)
{
    unsigned char i = 0;

    switchUsart2ToTx();
    USART2_TX_LED_ON();
    for(i=0; i<usart2TxCount; ++i) {
        TXREG2 = usart2TxBuffer[i];
        while(!TXSTA2bits.TRMT2);
    }
    usart2TxCount = 0;
    USART2_TX_LED_OFF();
    switchUsart2ToRx();
    
    return;
}


unsigned char convertHexToAsciiHi(unsigned char num) {
    num = num >> 4;
    if( num < 10 ) return num+48;
    else return num+55;
}

unsigned char convertHexToAsciiLo(unsigned char num) {
    num &= 0x0F;
    if( num < 10 ) return num+48;
    else return num+55;
}

char convertDecToHexInAsciiHi(int num) {
	return hexChars[ (num>>4) & 0x0F ];
}

char convertDecToHexInAsciiLo(int num) {
	return hexChars[ num & 0x0F ];
}
